﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Runtime.InteropServices;
using Microsoft.Robotics.Services.DepthCamSensor;
using Microsoft.Robotics.Services.Sensors.Kinect;
using depthcamsensor = Microsoft.Robotics.Services.DepthCamSensor.Proxy;

namespace NestedParticleFilter
{
    public class VectorFieldHistogram
    {
        #region Members

        #region Constants
        // you have to have the same Resolution with the Map
        // otherwise change the signature of WorldToMapX/ZCoordinate 
        private const double Resolution = Map.MapResolution;
        private const int WindowLength = 47; //33x33 cells for active_window
        private const int FieldOfView = 360;
        private const int Alpha = 5;  // This indicates the number of angles per sector
        private const int NumOfSectors = FieldOfView / Alpha;
        private static readonly double A = Math.Sqrt(2.0) * (WindowLength - 1.0) / 2.0;
        private static readonly double B = 1.0; 
        private const int ReachedThreshold = 20; // number of cells apart
        private  double ValleyThreshold = 0.01 ; // num of free sectors needed to pass a valley
        #endregion 

        private readonly double _mapWidth;
        private readonly double _mapHeight;
        private readonly int[,] _occMap; 
        private readonly Color[,] _map;
        
        private  List<double> _sectors; // list of sectors
        private List<int> _valleyStart;
        private List<int> _valleyEnd;
        

        private readonly int _row; // z
        private readonly int _column; //x
        private RobotPose _pose; //Cell based pose of the robot
        private Point _target;   // cell based pose of the target if any
        private Bitmap _madeMapImage; //map that the robot builds
        private Logger _log;
        #endregion

        private  readonly Color _backgroundColor = Color.LightGray;
        private  readonly Color _robotColor = Color.Black;
        
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="width">real width of the map of the environment in meters</param>
        /// <param name="height">real height of the map of the environment in meters</param>
        public VectorFieldHistogram(double width, double height)
        {
            _mapWidth = width*2; // multipling by two because initially the robot will be placed in the center of the map. Robot's location is unknown
            _mapHeight = height*2 ;
            //number of rows and columns
            _column = (int)Math.Ceiling(_mapWidth / Resolution);
            _row = (int)Math.Ceiling(_mapHeight / Resolution);
            // Ensure that the row has a multiple of 4 pixels,
            // i.e. it will be longword aligned when the bitmap
            // image is created
            if ((_column % 4) != 0)
                _column += 4 - (_column % 4);

            _madeMapImage = new Bitmap(_column,_row,PixelFormat.Format24bppRgb);
            _occMap = new int[_row, _column];
            _map = new Color[_row ,_column];
            for (int i = 0; i < _row; i++)
                for (int j = 0; j < _column; j++)
                    _occMap[i, j] = -1;

            _pose = new RobotPose(_column / 2.0, _row / 2.0, 0);
            _sectors = new List<double> ( NumOfSectors);
            _valleyStart = new List<int>();
            _valleyEnd = new List<int>();
            _log = new Logger(LogLevel.Error);

            for (int i = 0; i < _sectors.Capacity; i++ )
                _sectors.Add(0);
            Map.ClearMap(_map, _row, _column);

        }

        public Point Target
        {
            get { return _target; }
        }

        #region public methods

        /// <summary>
        /// Updates the map by adding the kinect reading and smoothing the sectors
        /// </summary>
        /// <param name="depthSate"></param>
        /// <param name="newPose"></param>
        /// <param name="rotationalSpeed"></param>
        public void UpdateMap (DepthCamSensorState depthSate, RobotPose newPose,out double rotationalSpeed)
        {
            ClearRobot();
           Map.ClearMap(_map, _row, _column);
           SetPose(newPose);
           //AddRobotToMap();
           AddKinectReading(depthSate);
           UpdateSectors();
           SmoothSectors();
           FindValley();
           GetParameters(out rotationalSpeed);
           UpdateMapData();
           MapToImage();
       }
       
        /// <summary>
        /// Locks the bitmap and returns the clone of the bitmap for multithread purposes.
        /// Since bitmap manipulation is not safe locking and cloning is necessary
        /// </summary>
        /// <returns></returns>
        public Bitmap GetMap ()
        {
            lock (_madeMapImage)
                return (Bitmap)_madeMapImage.Clone();
        }

        /// <summary>
        /// Sets the target destination for the robot to navigate to.
        /// </summary>
        /// <param name="target">target location in the map</param>
        public void SetTarget (Point target)
        {
            if (target == null)
            {
                _target = null;
                return;
            }
            double x = Map.WorldToMapXCoordinate(target.X,_column);
            double z = Map.WorldToMapZCoordinate(target.Z,_row);
            
            if (Map.ValidatePointInMap (x, z, _column,_row))
                _target = new Point(x,z);

        }
        /// <summary>
        /// Indicates if the robot has a target destination.
        /// </summary>
        /// <returns>returns true if the robot has a target destination, false otherwise</returns>
        public bool IsTargetSet()
        {
            if (_target != null)
                return true;
            return false;
        }
        /// <summary>
        /// Returns true if the robot has reached the target
        /// </summary>
        /// <returns></returns>
        public bool IsReached()
        {
            if (IsTargetSet())
            {
                double distance = GetDistance((int) _target.Z, (int) _target.X);
                _log.PrintMessage(LogLevel.Informative, "Distance from Target in Cells: " + distance);
                if (distance < ReachedThreshold)
                    return true;
            }
            return false;
            
        }
        
        #endregion

        #region Add Kinect Reading

        /// <summary>
        /// Adds the kinect reading to the map by increamenting the certainty value of each cell
        /// The certianty values are stored into the _occMap. The feild of view of the kinect
        /// is limited 57 degree feild of view
        /// </summary>
        /// <param name="depthState">Kinect Depth state information</param>
        private void AddKinectReading (DepthCamSensorState depthState )
        {
            const double angleStart = DepthAnalysis.FieldOfView / 2.0;
            const double maxRange = (double) KinectReservedSampleValues.MaxValidDepth; //4 meters
            
            int width = depthState.DepthImageSize.Width;
            var ranges = new short[width]; // maximum depth of each column is saved in this array
            int numRanges = width;
            double angleIncreament = (double)(DepthAnalysis.FieldOfView)/numRanges;

            // calculate the furthest "clear" distance for each column in the depth image);
            for (int column = 0; column < width; column++)
            {
               short depthInfo = DepthAnalysis.GetDepth(depthState.DepthImage, width, column, 180);
               ranges[column] = depthInfo;
               
            }
            //reverse the ranges because the kinect is from left to right, but we want right to left
            for (int i = 0; i < ranges.Length/2; i++)
            {
                short temp = ranges[i];
                ranges[i] = ranges[ranges.Length - 1 - i];
                ranges[ranges.Length - 1 - i] = temp;
            }
            // robots location 
            var rx = (int)_pose.X; // robots x in the grid
            var rz = (int)_pose.Z; // robots z in the grid
            // for every range calculate where it hitts
            for (int i = 0; i < numRanges; i++)
            {
                double range = ranges[i];
                // see if thr range is maxed
                bool maxed;
                if (range >= maxRange)// || range == 0)
                {
                    range = maxRange;
                    maxed = true;
                    Debug.WriteLine(range);
                }
                else
                    maxed = false; // hits the obstacle

               
                // Compute range end-point
                range = range / 1000; // convert it to meters

                double angle = -angleStart + i * angleIncreament;
                // anlge relative to the robot's pose
                angle += _pose.Theta; // in degree
                angle = UnitConversion.DegreeToRadian(angle); // in radian
                angle = UnitConversion.PutThetaInBound(angle);

                // calculate the actual coordinate where the laser hits
                int laserz = -(int) Math.Floor(range/Resolution*Math.Cos(angle)) + rz;
                int laserx = -(int) Math.Floor(range/Resolution*Math.Sin(angle)) + rx;

                int stepCount = (int) Math.Floor(range/ Resolution) + 2;
               
                double dx = (laserx - rx);
                double dz = (laserz - rz);
                dx /= (stepCount - 1);
                dz /= (stepCount - 1);
                int j = 0;
                for (j = 0; j < stepCount; j++)
                {
                    int px = rx + (int) (dx*j);
                    int pz = rz + (int) (dz*j);
                    // if the cell has not been analyzed in this run , and is a valid cell
                    if ( Map.ValidatePointInMap(px, pz, _column, _row))
                    {
                        _occMap[pz, px]--;
                        if (_occMap[pz, px] < 0) // if it was previously uknown, set it as it is free
                        {
                            _occMap[pz, px] = 0; // free
                        }
                       
                    }
                        
                } //end of for*/
                // Place an obstacle at the end of the ray if the scan
                // was not maxed out (a "miss" returns the max range value)
                if (!maxed)
                {
                    if (Map.ValidatePointInMap(laserx, laserz, _column,_row) 
                        && _occMap[laserz,laserx] < 100)
                        _occMap[laserz, laserx]++;

                } //end of !maxed


                

            } //end of for

        }

        #endregion

        #region Map related Methods

        /// <summary>
        /// Makes a bitmap based on the 2D color map informatiom
        /// </summary>
        private void MapToImage()
        {
            lock (_madeMapImage)
            {
                BitmapData bmpData =
                    _madeMapImage.LockBits(new Rectangle(0, 0, _madeMapImage.Width, _madeMapImage.Height),
                                           ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);

                IntPtr ptr = bmpData.Scan0;
                int numBytes = Math.Abs(bmpData.Stride) * _madeMapImage.Height;
                byte[] rgbValues = new byte[numBytes];
                Marshal.Copy(ptr, rgbValues, 0, numBytes);

                // Set every third value to 255. A 24bpp bitmap will look red.  
                int counter = 2;
                for (int i = 0; i < _row; i++)
                    for (int j = 0; j < _column; j++)
                    {
                        if (_occMap[i, j] > 0)
                        {
                            rgbValues[counter] = (byte)_occMap[i, j];
                            rgbValues[counter - 1] = 0;
                            rgbValues[counter - 2] = 0;
                        }
                        else if (_occMap[i, j] == 0)
                        {
                            rgbValues[counter] = 0;
                            rgbValues[counter - 1] = 255;
                            rgbValues[counter - 2] = 0;
                        }
                        else if (_occMap[i, j] < 0)
                        {
                            rgbValues[counter] = 255;
                            rgbValues[counter - 1] = 255;
                            rgbValues[counter - 2] = 255;
                        }
                        counter += 3;
                    }

                //const int length = (int)((WindowLength / 2.0));
                //for (var i = (int)(_pose.Z - length); i < _pose.Z + length && i < _row; i++)
                //    for (var j = (int)(_pose.X - length); j < _pose.X + length && j < _column; j++)
                //    {
                //        int index = GetSector(i, j);
                //        counter = (i * _madeMapImage.Width + j) * 3;
                //        if (index < NumOfSectors)
                //            if (_sectors[index] == 0)
                //            {
                //                rgbValues[counter] = 255;
                //                rgbValues[counter - 1] = 255;
                //                rgbValues[counter - 2] = 0;
                //            }

                //            else if (_sectors[index] > 0)
                //            {
                //                rgbValues[counter] = 0;
                //                rgbValues[counter - 1] = 0;
                //                rgbValues[counter - 2] = 255;
                //            }

                //    }

                // Copy the RGB values back to the bitmap
                Marshal.Copy(rgbValues, 0, ptr, numBytes);

                //// Unlock the bits.
                _madeMapImage.UnlockBits(bmpData);
            }

            //lock (_madeMapImage)
            //{

            //    if (_madeMapImage != null && _map != null)

            //        for (int i = 0; i < _row; i++)
            //            for (int j = 0; j < _column; j++)
            //            {

            //                _madeMapImage.SetPixel(j, i, _map[i, j]);

            //            }

            //}           
        }

        /// <summary>
        /// update the color map based on the laser / depth beam based on the occupancy map
        /// </summary>
        private void UpdateMapData()
        {


            for (int k = 0; k < _row; k++)
                for (int j = 0; j < _column; j++)
                {
                    if (_occMap[k, j] > 0) // if the cell is known to be occupied color it red
                    {
                        _map[k, j] = Color.FromArgb(_occMap[k, j], 0, 0);
                    }
                    else if (_occMap[k, j] == 0) // if the cell is free color it beige
                    {
                        _map[k, j] = Color.ForestGreen;
                    }
                }

            //const int length = (int)((WindowLength / 2.0));
            //for (var i = (int)(_pose.Z - length); i < _pose.Z + length && i < _row; i++)
            //    for (var j = (int)(_pose.X - length); j < _pose.X + length && j < _column; j++)
            //    {
            //        int index = GetSector(i, j);
            //        if (index < NumOfSectors && index >= 0)
            //            if (_sectors[index]< ValleyThreshold)
            //                _map[i, j] = Color.Yellow;
            //            else if (_sectors[index] > ValleyThreshold)
            //                _map[i, j] = Color.Red;
            //    }
            
            
            //AddRobotToMap();

        }

        #endregion 

        #region Sectors

        /// <summary>
        /// As the robot moves, the active widnow moves with the robot, Active window is centered around
        /// the robot's position. This function saves a copy of the certainty value of each grid that are
        /// included in active window inside the active window array.
        /// It then calculate the obstacle_vector for that grid and looks up the sector value of that grid.
        /// It saves the obstacle vector value for the specified sector and smooth the sectors at the end.
        /// Called at addLaser function
        /// </summary>
        private void UpdateSectors()
        {
            double obstacleVector = 0;
            ClearSectors();
            _log.PrintMessage(LogLevel.Informative,"\n-------------------------------");
            const double length = WindowLength / 2.0;
            for (var i = (int)(_pose.Z - length); i < _pose.Z + length && i < _row && i >= 0; i++)
                for (var j = (int)(_pose.X - length);
                    j < _pose.X + length && j < _column && j >= 0; j++)
                {
                    // Debug.WriteLine("i,j: " +i + ", " + j + " value: " + _occMap[i, j]);
                    if (_occMap[i, j] >= 0)
                    {
                        obstacleVector = Math.Pow(_occMap[i, j], 2) * (A - B * (GetDistance(i, j)));
                    }
                    //Debug.WriteLine("Obstacle Vector: "+ obstacleVector);
                    int index = GetSector(i, j);
                    //Debug.WriteLine("Index: "+ index);
                    if (index < _sectors.Count && index >= 0)
                        _sectors[index] += obstacleVector;
                }
            //PrintSectors();
        }

        /// <summary>
        /// clears all the sectors 
        /// </summary>
        private void ClearSectors()
        {
            for (int i = 0; i < _sectors.Count; i++)
                _sectors[i] = 0;
        }


        /// <summary>
        /// Smooths out the sector based on the formula in VFH
        /// h_k = 1h_(k-l)+ 2*h_(k-l+1)+... + h_k+...+2h_(k+l-1)+h_(k+l)
        /// </summary>
        /// <returns>smooted histogram </returns>
        private void SmoothSectors()
        {
            lock (_sectors)
            {
                double sum = 0;
                for (int i = 0; i < _sectors.Count; i++)
                    sum += _sectors[i];
                if (sum == 0)
                    return;
                for (int i = 0; i < _sectors.Count; i++)
                    _sectors[i] /= sum;
            }
            _log.PrintMessage(LogLevel.Informative, "----------------------Smoothed--------------");
           // PrintSectors();
            
            //int factor = 3;
            //var newSectors = new List<double> (_sectors.Count);

            //for (int i = 0; i < _sectors.Count; i++)
            //{
            //    int count;
            //    double sum = factor * _sectors[i]; // 5*hk
            //    // for each current sector, adds the sector to the left and right
            //    int left = i - 1;
            //    if (left == -1)
            //        left = _sectors.Count - 1;
            //    count = factor - 1;
            //    do
            //    {
            //        sum += (count) * _sectors[left];
            //        count--;
            //        left--;
            //        if (left < 0)
            //            left = _sectors.Count - 1;
            //    } while (count != 0);

            //    int right = i + 1;
            //    if (right == _sectors.Count)
            //        right = 0;
            //    count = factor - 1;
            //    do
            //    {
            //        sum += (count) * _sectors[right];
            //        count--;
            //        right++;
            //        if (right >= _sectors.Count)
            //            right = 0;
            //    } while (count != 0);
            //    newSectors[i] = sum / (2 * factor  -1);
            //}
            //Debug.WriteLine("New Sectors");
            //for (int i = 0; i < _sectors.Count; i++)
            //    Debug.Write(newSectors[i]+ " ");
            //Debug.WriteLine(" ");
            //return newSectors;
        }

        private void FindValley ()
        {
            // clear the list
            _valleyEnd.Clear();
            _valleyStart.Clear();
            int i = 0, j = 0;
            // find intervals suitablel for manuvare
           while ( i < NumOfSectors)
           {
               if (_sectors[j] < ValleyThreshold) // if its free
                   j++;
               else if ( i != j  && j-i >1)
               {
                   _valleyStart.Add(i);
                   _valleyEnd.Add(j-1);
                   i = j+1;
               }
               else // its occupied
               {
                   i++;
                   j = i;
               }
               if ( j >= NumOfSectors && j > i&& j-i >1 )
               {
                   _valleyEnd.Add(j-1);
                   _valleyStart.Add(i);
                   i = j;
               }
               else if (j >= NumOfSectors && j > i)
               {
                   i++;
               }
           }
            int numOfValleis = _valleyEnd.Count;
            if (numOfValleis > 1 && _valleyStart[0] ==0 && _valleyEnd[numOfValleis-1] == NumOfSectors-1)
            {
                _valleyStart[0] = _valleyStart[numOfValleis - 1];
                _valleyStart.RemoveAt(numOfValleis-1);
                _valleyEnd.RemoveAt(numOfValleis-1);
            }


           Debug.Assert(_valleyStart.Count == _valleyEnd.Count, "We cannot have mismatched beginning and ending for valleys! ");          
           // print valleys
           //PrintValleys();
          
        }

        /// <summary>
        /// Returns the rotational velocity (omega) based on where the robot is and 
        /// the target is
        /// </summary>
        /// <param name="rotationalSpeed"></param>
        private void GetParameters ( out double rotationalSpeed)
        {
            rotationalSpeed = 0;
            _log.PrintMessage(LogLevel.Informative, "Center Point:\t\t\t\t\t\t"+ _pose.ToString());
            if (_target != null)
            {
                _log.PrintMessage(LogLevel.Informative, "Target Point:\t\t\t\t\t\t" + _target.ToString());
                int targetSector = GetSector((int) _target.Z, (int) _target.X); //sector of the target
                _log.PrintMessage(LogLevel.Informative, "Target Sector:                                \t\t" + targetSector);
                
                double robotTheta = 0;
                int m ,steeringSector = 0;
                double minDistance= 73;
                int candidIntervalIndex = 0; // candidn interval index
                int closeToStartOrEnd=0;
                
                // find the closest sector to the target
                for (int i = 0; i < _valleyStart.Count; i++)
                {
                 
                    int a = Math.Abs(targetSector - _valleyStart[i]+1);
                    int b = Math.Abs(targetSector - _valleyEnd[i]+1);
                    minDistance = Math.Min(a, Math.Min(minDistance, b));
                    if (minDistance == a || minDistance == b)
                        candidIntervalIndex = i;
                    if (minDistance == a)
                        closeToStartOrEnd = 0;
                    else if (minDistance == b)
                        closeToStartOrEnd = 1;
                   
                }

                _log.PrintMessage(LogLevel.Informative, "Closest Sector:\t\t\t\t\t\t [" + _valleyStart[candidIntervalIndex] + ", " + _valleyEnd[candidIntervalIndex] + "]");

                
                int differance =  _valleyEnd[candidIntervalIndex] - _valleyStart[candidIntervalIndex] ;
                _log.PrintMessage(LogLevel.Informative, "Difference btw valley_end and valley_start:  \t\t" + differance);
                // if the valleyEnd is less than the valleyStart index ex: [69-3] i.e. cercular situation, difference will be negative 
                // &&  if the target is in the free sector intervals
                if (differance < 0 && 
                   ( targetSector <= _valleyEnd[candidIntervalIndex] || 
                    targetSector >= _valleyStart[candidIntervalIndex]))
                {
                    //steeringSector = _valleyStart[candidIntervalIndex]+ 
                    //    (NumOfSectors - _valleyStart[candidIntervalIndex] + _valleyEnd[candidIntervalIndex] + 1) / 2;
                    steeringSector = targetSector;
                    // incase the start is greater than the end valley, steering_sector will be greater than NumOfSectors
                    if (steeringSector >= NumOfSectors)
                        steeringSector -= NumOfSectors;
                    _log.PrintMessage(LogLevel.Informative, "Target lays in the free interval.[Special Case] Steering_sector to face " + steeringSector);
                   
                }
                else if (differance > 0
                    && targetSector <= _valleyEnd[candidIntervalIndex]
                    && targetSector >= _valleyStart[candidIntervalIndex])
                {
                    //steeringSector = (_valleyStart[candidIntervalIndex] + _valleyEnd[candidIntervalIndex] + 1) / 2;
                    steeringSector = targetSector;
                    _log.PrintMessage(LogLevel.Informative, "Target lays in the free interval.[Normal Case] Steering_sector to face " + steeringSector);
                  
                }
                else
                {
                    steeringSector = closeToStartOrEnd == 0
                                          ? _valleyStart[candidIntervalIndex] + 2
                                          : _valleyEnd[candidIntervalIndex] - 2;
                    _log.PrintMessage(LogLevel.Informative, "Target falls into occupied interval. Steering_sector to face " + steeringSector);
                  
                }
                robotTheta = _pose.Theta;
                if (robotTheta < 0)
                   robotTheta = 360 + robotTheta;
                int robotSector = (int)(robotTheta / Alpha);
                _log.PrintMessage(LogLevel.Informative, "Robot's sector: " + robotSector);
                int diff1 = PutSectorInBound( NumOfSectors - steeringSector + robotSector);
                int diff3 = robotSector - steeringSector;

                m = Math.Abs(diff3) > (NumOfSectors / 2) ? diff1 : diff3;

                

                _log.PrintMessage(LogLevel.Informative, "m : " + m);
                rotationalSpeed = (double)m / (NumOfSectors/2);
                if (rotationalSpeed > 1)
                    rotationalSpeed = 1 - rotationalSpeed;

                _log.PrintMessage(LogLevel.Informative, "Turning Factor: " + rotationalSpeed);
                   
               }
          
            
        }

        #endregion

        #region Get Distance/Sector/Angle 
        /// <summary>
        /// Distance in cells between the robot and the specified cell
        /// </summary>
        /// <param name="row">row of the cell (z)</param>
        /// <param name="col">col of the cell (x) </param>
        private double GetDistance(int row, int col)
        {
            return
                Math.Sqrt(Math.Pow(_pose.Z - row, 2) + Math.Pow(_pose.X - col, 2));

        }

        /// <summary>
        /// Gets the sector of a specified cell
        /// </summary>
        /// <param name="row">row of the cell</param>
        /// <param name="col"> col of the cell </param>
        /// <returns>sector of the cell </returns>
        private int GetSector(int row, int col)
        {
            double angle = GetAngle(row, col);
            var sector = (int)(angle / Alpha);
            Debug.Assert(sector < NumOfSectors && sector >=0, "Out of range sector");
            return sector;
        }

        /// <summary>
        /// Returns the direction from active cell (z,x) to the robot center point
        /// </summary>
        /// <param name="z">row of the cell</param>
        /// <param name="x">column of the cell</param>
        /// <returns>angle in degree</returns>
        private double GetAngle(int z, int x)
        {
            double numerator = _pose.X - x;
            double denominator = _pose.Z - z;

            if (denominator == 0)
                denominator = 0.001;

            double angle = Math.Atan2(numerator, denominator);
            angle = UnitConversion.RadianToDegree(angle);
           // angle -= _pose.theta;
            //angle += (FieldOfView/2); // this is added since the kinect field of view is limited

            if (angle <0)
                angle = 360 + angle;
            
            return angle;
        }

        #endregion

        #region Robot Related Method

        /// <summary>
        /// Sets the robot's pose. The x and z are CELL BASED pose and not real world coordinates
        /// </summary>
        /// <param name="newPose">world based Robot's new location </param>
         private void SetPose(RobotPose newPose)
        {
            _pose.X = Map.WorldToMapXCoordinate(newPose.X, _column);
            _pose.Z = Map.WorldToMapZCoordinate(newPose.Z, _row);
            _pose.Theta = newPose.Theta;
        }


        /// <summary>
        /// Add a single robot to the map
        /// </summary>
        private void AddRobotToMap()
        {
            // clear the traps of the robot.
            ClearRobot();
            int angle = Map.WorldToMapTheta(_pose.Theta);
            // draw robot 
            Map.DrawRobot((int)_pose.X, (int) _pose.Z, angle, _map, _robotColor, _row, _column);
          
        }

        /// <summary>
        /// Clears the robot from the map
        /// </summary>
        private void ClearRobot()
        {
            for (int i = 0; i < _row; i++)
                for (int j = 0; j < _column; j++)
                {
                    if (_map[i, j] == _robotColor)
                        _map[i, j] = _backgroundColor;
                }
        }

        #endregion

        public void PrintValleys()
        {
            for (int k = 0; k < _valleyStart.Count; k++)
            {

                _log.PrintMessage(LogLevel.Informative, "[" + _valleyStart[k] + ", " + _valleyEnd[k] + "] ");
            }
            
        }
        public void PrintSectors()
        {
            for (int i = 0; i < _sectors.Count; i++)
                _log.PrintMessage(LogLevel.Informative, i + "(" + (i * 5) + "): " + _sectors[i] + " ");
            _log.PrintMessage(LogLevel.Informative, "\n");
        }
       
        public int PutSectorInBound (int sector)
        {
    
                if (sector < 0)
                    sector += NumOfSectors;
                if (sector> NumOfSectors)
                    sector = sector - NumOfSectors;   
            return sector; 
        }   
    }
}
